//------------------------------------------------
//		SERVO.c
//	SERVO library
//	Arthur Benemann 19/12/2011
//------------------------------------------------
//	Description:
//FIXME
//-----------------------------------------------
//	Requeriments:
//	OCx
//	1 Remapable pin (SERVO out)
//-----------------------------------------------

#include "picDev.h"

//----------------------------------------------
void initSERVO(){
    SERVO1_OUT= Ro_OC1;

    T5CON = 0x8010;     // Timer 5 ON, Internal OSC, 1:8 prescaler , 0.5usec tick
    PR5 = 2000;         // Timer period

    OC1R  = 1500*2u;    // Starting servo value 1.5ms
    OC1RS = 20000*2u; 	// Period 20ms
    OC1CON2 = 0x101f; // Timer 5 as Source
    OC1CON1 = 0x0C07;
	
}

//----------------------------------------------
void writeSERVO(unsigned char ch, unsigned int position)
{
	OC1R  = position *2u;				// Duty cycle
}
